
void translateForwardDistanceBased(int motorPower, int distanceValue);

void translateBackwardsDistanceBased(int motorPower, int distanceValue);

void translateLeftDistanceBased(int motorPower, int distanceValue);

void translateRightDistanceBased(int motorPower, int distanceValue);

void rotateClockwiseAngleBased(int motorPower, int angle);

void halt();

void movement(int scaledX ,int scaledY,int scaledTheta);

void toggleHook();

int getEncoderValueForAngle(int angleDegrees);

int getEncoderValueForDistance(int distanceInches);

//task hookPosition();

const int CONVERSION_1_INCH = 91;  //constants for converting distance to encoders
const int CONVERSION_1_DEGREE = 9;

const int SERVO_LEFT_UP = 0;
const int SERVO_LEFT_MID = 0;
const int SERVO_LEFT_DOWN = 0;
const int SERVO_RIGHT_UP = 0;
const int SERVO_RIGHT_MID = 0;
const int SERVO_RIGHT_DOWN = 0;

typedef enumWord
{
	Down, Up, Mid
} HookPosition;

HookPosition servoLeft = Up;
HookPosition servoRight = Up;

//task hookPosition()
//{
//	while (true)
//	{
//		if (ServoValue[Hook] == SERVO_LEFT_UP)
//		{
//			servoLeft = Up;
//		}
//		else if (ServoValue[Hook] == SERVO_LEFT_MID)
//		{
//			servoLeft = Mid;
//		}
//		else if (ServoValue[Hook] == SERVO_LEFT_DOWN)
//		{
//			servoLeft = Down;
//		}

//		if (ServoValue[Hook] == SERVO_RIGHT_UP)
//		{
//			servoRight = Up;
//		}
//		else if (ServoValue[Hook] == SERVO_RIGHT_MID)
//		{
//			servoRight = Mid;
//		}
//		else if (ServoValue[Hook] == SERVO_RIGHT_DOWN)
//		{
//			servoRight = Down;
//		}
//	}
//	EndTimeSlice();
//}


void movement(int scaledX ,int scaledY,int scaledTheta)//Moves the robot based on joystick values.
{
	motor[Front] = (scaledY + scaledX - scaledTheta);
	motor[Right] = (-scaledY + scaledX - scaledTheta);
	motor[Left] = (scaledY + -scaledX - scaledTheta);
	motor[Rear] = (-scaledY + -scaledX - scaledTheta);
}

void rotateCounterClockwiseAngleBased(int motorPower, int angle);void halt() // stops all drive motors
{
	motor[Left] = 0;
	motor[Right] = 0;
	motor[Front] = 0;
	motor[Rear] = 0;
}

int getEncoderValueForAngle(int angleDegrees) // converts an angle to an encoder value
{
	return angleDegrees * CONVERSION_1_DEGREE;
}

int getEncoderValueForDistance(int distanceInches)//Converts a distance in inches to an encoder value.
{
	return distanceInches * CONVERSION_1_INCH;
}

void translateForwardDistanceBased(int motorPower, int distanceValue)//Moves the robot forward at a set power for a distance.
{
	nMotorEncoder[Right] = 0;
	motor[Left] = motorPower;
	motor[Right] = motorPower * -1;
	while (abs(nMotorEncoder[Right]) < getEncoderValueForDistance(distanceValue))
	{
	}
	halt();
}

void translateBackwardsDistanceBased(int motorPower, int distanceValue)//Translates the robot backwards using encoders.
{
	nMotorEncoder[Right] = 0;
	motor[Left] = motorPower * -1;
	motor[Right] = motorPower;
	while (abs(nMotorEncoder[Right]) < getEncoderValueForDistance(distanceValue))
	{

	}
	halt();
}

void translateLeftDistanceBased(int motorPower, int distanceValue)//Translates the robot left using encoders.
{
	nMotorEncoder[Rear] = 0;
	motor[Front] = motorPower * -1;
	motor[Rear] = motorPower;
	while (abs(nMotorEncoder[Rear]) < getEncoderValueForDistance(distanceValue))
	{

	}
	halt();
}

void translateRightDistanceBased(int motorPower, int distanceValue)//Translates the robot right using encoders.
{
	nMotorEncoder[Rear] = 0;
	motor[Front] = motorPower;
	motor[Rear] = motorPower * -1;
	while (abs(nMotorEncoder[Rear]) < getEncoderValueForDistance(distanceValue))
	{

	}
	halt();
}

void rotateClockwiseAngleBased(int motorPower, int angle)//Rotates the robot clockwise using encoders.
{
	nMotorEncoder[Right] = 0;
	motor[Left] = motorPower;
	motor[Right] = motorPower;
	motor[Front] = motorPower;
	motor[Rear] = motorPower;
	while (abs(nMotorEncoder[Right]) < getEncoderValueForAngle(angle))
	{
	}
	halt();
}

void rotateCounterClockwiseAngleBased(int motorPower, int angle)//Rotates the robot counter-clockwise using encoders.
{
	nMotorEncoder[Right] = 0;
	motor[Left] = motorPower * -1;
	motor[Right] = motorPower * -1;
	motor[Front] = motorPower * -1;
	motor[Rear] = motorPower * -1;
	while (abs(nMotorEncoder[Right]) < getEncoderValueForAngle(angle))
	{

	}
	halt();
}

void toggleHook()//Toggles the position of the hook servo
{
	if(servoLeft == Down )//Resets hook
	{
		servo[Hook] = SERVO_LEFT_MID;
		servo[Hook2] = SERVO_RIGHT_MID;
		//while (!(ServoLeft == Mid)
		//{
		//}
		servoLeft = Mid;
		servoRight = Mid;
		servo[Hook] = SERVO_LEFT_UP;
		servo[Hook2] = SERVO_RIGHT_UP;
		//while (!(ServoLeft == Up)
		//{
		//}
		servoLeft = Up;
		servoRight = Up;
	}
	else//Puts hook down
	{
		servo[Hook] = SERVO_LEFT_DOWN;
		servo[Hook2] = SERVO_RIGHT_DOWN;
		servoLeft = Down;
		servoRight = Down;
	}
}
